#include "Polygon_containment.h"

namespace Field_packing
{
	const double Polygon_containment::pi = boost::math::constants::pi<double>();
	void Quadratic_function::extrema(double min_x, double max_x, double min_y, double max_y, double& fmin, double& fmax) const
	{
		fmin = std::numeric_limits<double>::max();
		fmax = -std::numeric_limits<double>::max();
		// 1. the global optimal
		double det = 4*A*B - C*C;
		if (det > 0) // ths should be always executed for the polygon containment problem
		{
			double optimal_x = (C*E - 2*B*D)/det;
			double optimal_y = (C*D - 2*A*E)/det;
			if ( within_range(min_x, max_x, optimal_x) && within_range(min_y, max_y, optimal_y) )
			{
				if (A > 0) fmin = (*this)(optimal_x, optimal_y);
				else       fmax = (*this)(optimal_x, optimal_y);
			}
		}	
		// 2. on the boundary
		double f_min_x_min_y = (*this)(min_x, min_y), f_min_x_max_y = (*this)(min_x, max_y);
		double f_max_x_min_y = (*this)(max_x, min_y), f_max_x_max_y = (*this)(max_x, max_y);
		fmin = std::min(fmin, f_min_x_min_y);
		fmin = std::min(fmin, f_min_x_max_y);
		fmin = std::min(fmin, f_max_x_min_y);
		fmin = std::min(fmin, f_max_x_max_y);
		fmax = std::max(fmax, f_min_x_min_y);
		fmax = std::max(fmax, f_min_x_max_y);
		fmax = std::max(fmax, f_max_x_min_y);
		fmax = std::max(fmax, f_max_x_max_y);
		double min_x_opti_y = -(C*min_x + E)/(2*B), max_x_opti_y = -(C*max_x + E)/(2*B);
		double opti_x_min_y = -(C*min_y + D)/(2*A), opti_x_max_y = -(C*max_y + D)/(2*A);
		// 2.1 boundary {(x, y) | x = min_x, min_y <= y <= max_y} 
		if (within_range(min_y, max_y, min_x_opti_y))
		{
			double f_min_x_opti_y = (*this)(min_x, min_x_opti_y);
			fmin = std::min(fmin, f_min_x_opti_y);
			fmax = std::max(fmax, f_min_x_opti_y);;
		}
		// 2.2 {(x, y) | x = max_x, min_y <= y <= max_y}
		if (within_range(min_y, max_y, max_x_opti_y))
		{
			double f_max_x_opti_y = (*this)(max_x, max_x_opti_y);
			fmin = std::min(fmin, f_max_x_opti_y);
			fmax = std::max(fmax, f_max_x_opti_y);
		}
		// 2.3 {(x, y) | y = min_y, min_x <= x <= max_x}
		if (within_range(min_x, max_x, opti_x_min_y))
		{
			double f_opti_x_min_y = (*this)(opti_x_min_y, min_y);
			fmin = std::min(fmin, f_opti_x_min_y);
			fmax = std::max(fmax, f_opti_x_min_y);
		}
		// 2.4 {(x, y) | y = max_y, min_x <= x <= max_x}
		if (within_range(min_x, max_x, opti_x_max_y))
		{
			double f_opti_x_min_y = (*this)(opti_x_max_y, max_y);
			fmin = std::min(fmin, f_opti_x_min_y);
			fmax = std::max(fmax, f_opti_x_min_y);
		}
	}
	int Polygon_containment::solve_sine(double a, double b, double c, double& root_1, double& root_2)
	{
		if (c < -1.0 || c > 1.0) // there is no intersection points between the ellipse and the segment
			return 0;
		assert(b - a < pi);
		//assert(c >= -1.0 && c <= 1.0);
		root_1 = std::asin(c), root_2 = pi - root_1;
		// -pi/2 + 2*n*pi <= a <= root <= b <= 3*pi/2 + 2*n*pi
		bool root_1_valid = false, root_2_valid = false;
		int nb_root = 0;
		double fn = (a - root_1)/(2*pi);
		int n = static_cast<int>(std::ceil(fn));
		double fm = (b - root_1)/(2*pi);
		int m = static_cast<int>(std::floor(fm));
		if (m == n)
		{
			root_1 += 2*n*pi;
			nb_root++;
			root_1_valid = true;
		}
		fn = (a - root_2)/(2*pi);
		n = static_cast<int>(std::ceil(fn));
		fm = (b - root_2)/(2*pi);
		m = static_cast<int>(std::floor(fm));
		if (m == n)
		{
			root_2 += 2*n*pi;
			nb_root++;
			root_2_valid = true;
		}
		if (!root_1_valid && root_2_valid)
			root_1 = root_2;
		return nb_root;
	}
	std::pair<double, size_t> Polygon_containment::solve_eqn(std::function<double(double)> func_val, std::function<double(double)> diff_val, double init_x, double stop_error, size_t max_iter_nb)
	{
		double curx = init_x, prevx;
		size_t nb_iter = 0;
		do
		{
			prevx = curx;
			curx = curx - func_val(curx)/diff_val(curx);
			nb_iter++;
		} while (std::fabs(curx - prevx) >= stop_error && nb_iter <= max_iter_nb);
		return std::pair<double, size_t>(curx, nb_iter);
	}
	bool Polygon_containment::is_clamped(const Segment_2& fixed_edge_1, const Segment_2& fixed_edge_2, const Segment_2& moved_edge)
	{
		Point_2 P1 = fixed_edge_1.source(), P2 = fixed_edge_1.target();
		Point_2 Q1 = fixed_edge_2.source(), Q2 = fixed_edge_2.target();
		// coefficients of the quadratic form
		Vector_2 P1P2(P1, P2), Q1Q2(Q1, Q2), P1Q1(P1, Q1);
		double A = P1P2.squared_length(), B = Q1Q2.squared_length();
		double C = -2*(P1P2*Q1Q2);
		double D = -2*(P1P2*P1Q1), E = 2*(Q1Q2*P1Q1);
		double F = P1Q1.squared_length();
		// compute the min and max of 
		// f(alpha, beta) = A*alpha^2 + B*beta^2 + C*alpha*beta + D*alpha + E*beta + F
		// on the range of 0<=alpha<=1 and 0<=beta<=1
		Quadratic_function qf(A, B, C, D, E, F);
		double max_len2, min_len2;
		qf.extrema(0.0, 1.0, 0.0, 1.0, min_len2, max_len2);
		double PQ_len2 = moved_edge.squared_length();
		if (min_len2 <= PQ_len2 && PQ_len2 <= max_len2)
			return true;
		else
			return false;
	}
	int Polygon_containment::type_I_contact_detect( 
													const Segment_2& AB, const Segment_2& BC, const Segment_2& PQ, double alpha,
													double& root_1, double& root_2
												  )
	{
		// compute the angle alpha and build the coordinate axis
		Point_2 A = AB.source(), B = AB.target(), C = BC.target();
		// a*sin(x) + b*cos(x) = c*sin(x+phi), where c = sqrt(a^2 + b^2), tan(phi) = b/a
		double len_AB = CGAL::sqrt(AB.squared_length());
		double len_BC = CGAL::sqrt(BC.squared_length());
		// the expression of x_C(theta) and y_C(theta)
		double tan_alpha = std::tan(alpha);
		//double cos_beta = AB.to_vector()*BC.to_vector()/(std::sqrt(AB.squared_length()*BC.squared_length()));
		//double sin_beta = std::sqrt(1.0 - cos_beta*cos_beta);
		double cos_beta, sin_beta;
		compute_sin_cos(AB.to_vector(), BC.to_vector(), sin_beta, cos_beta);
		double ax =	0.5*len_AB/tan_alpha - len_BC*sin_beta, bx = 0.5*len_AB + len_BC*cos_beta;
		double ay = 0.5*len_AB + len_BC*cos_beta, by = 0.5*len_AB*tan_alpha + len_BC*sin_beta;

		//for (CGAL_polygon::Edge_const_iterator eit = exterior.edges_begin(); eit != exterior.edges_end(); ++eit)
		//{
		Point_2 P = PQ.source(), Q = PQ.target();
		double coeff_sin = ax*(Q.y() - P.y()) + bx*(P.x() - Q.x());
		double coeff_cos = ay*(Q.y() - P.y()) + by*(P.x() - Q.x());
		double comp_coeff = std::hypot(coeff_sin, coeff_cos);
		double phi = lincomb_phi(coeff_sin, coeff_cos); // shifted phrase
		//double sin_phi = coeff_cos/comp_coeff, cos_phi = coeff_sin/comp_coeff;
		double rhs = P.x()*Q.y() - Q.x()*P.y(); // comp_coeff*sin(theta + phi) = rhs
		int nb_root = solve_sine(-alpha + phi, alpha + phi, rhs/comp_coeff, root_1, root_2);
		if (nb_root == 0)
			return 0;
		// check whether the intersection point is on the segment
		std::function<bool(double)> on_segment = [=](double root) -> bool
													{
														double sn = std::sin(root), cn = std::cos(root);
														double x = ax*sn + ay*cn, y = bx*sn + by*cn;
														Point_2 ipnt(x, y);
														return Vector_2(ipnt, P)*Vector_2(ipnt, Q) <= 0;
													};
		// for debug
		std::function<double(double)> collinear_check = [=](double root) -> double
														{
															double sn = std::sin(root), cn = std::cos(root);
															double x = ax*sn + ay*cn, y = bx*sn + by*cn;
															Point_2 ipnt(x, y);
															Vector_2 ip(ipnt, P), iq(ipnt, Q);
															return std::fabs(ip.x()*iq.y() - ip.y()*iq.x());
														};
		if (nb_root == 2)
		{
			root_1 -= phi;
			root_2 -= phi;
			bool root_1_valid = on_segment(root_1);
			bool root_2_valid = on_segment(root_2);
			if (!root_1_valid)
				nb_root--;
			if (!root_2_valid)
				nb_root--;
			if (!root_1_valid && root_2_valid)
				root_1 = root_2;
		}
		else //if (nb_root == 1)
		{
			root_1 -= phi;
			if (!on_segment(root_1))
				nb_root--;
			//else // for debug
			//	std::cout<<collinear_check(root_1)<<std::endl;
		}
		return nb_root;
		//}
	}
	
	int Polygon_containment::type_II_contact_detect(
													const Local_frame& lf, const Point_2& w, const Segment_2& AB, const Segment_2& dynamic_edge, 
													double alpha, std::list<double>& roots
												   )
	{
		const Point_2 P = dynamic_edge.source();
		const Point_2 Q = dynamic_edge.target();
		// the angles <BA, BP> and <BA, BQ>
		//Vector_2 BA = AB.opposite().to_vector();
		Vector_2 BP(AB.target(), P), BQ(AB.target(), Q);
		double len_BP = std::sqrt(BP.squared_length()), len_BQ = std::sqrt(BQ.squared_length()), len_AB = std::sqrt(AB.squared_length()), len_PQ = std::sqrt(dynamic_edge.squared_length());
		//double cos_beta_ABP = BA*BP/(len_AB*len_BP), cos_beta_ABQ = BA*BQ/(len_AB*len_BQ);
		//double sin_beta_ABP = std::sqrt(1.0 - cos_beta_ABP*cos_beta_ABP), sin_beta_ABQ = std::sqrt(1.0 - cos_beta_ABQ*cos_beta_ABQ);
		double cos_beta_ABP, cos_beta_ABQ, sin_beta_ABP, sin_beta_ABQ;
		compute_sin_cos(AB.to_vector(), BP, sin_beta_ABP, cos_beta_ABP);
		compute_sin_cos(AB.to_vector(), BQ, sin_beta_ABQ, cos_beta_ABQ);
		double coeff_cos = len_BQ * sin_beta_ABQ - len_BP * sin_beta_ABP, coeff_sin = len_BQ * (cos_beta_ABQ) - len_BP * (cos_beta_ABP);
		double phi_y = lincomb_phi(coeff_sin, coeff_cos);
		double phi_x = lincomb_phi(-coeff_cos, coeff_sin);
		// construct the trigonometric equation
		std::function<double(double)> eqn_eval, eqn_diff;
		Point_2 lw = lf.to_local_coord(w);

		double phi_w = lincomb_phi(-lw.x(), lw.y());
		double a = 0.5*len_AB/std::sin(2*alpha);
		double b = -std::hypot(lw.x(), lw.y());
		// -0.5*len_AB/std::sin(2*alpha - phi_y) + len_BP * (sin_beta_ABP * coeff_sin / len_PQ - (-cos_beta_ABP) * coeff_cos / len_PQ)
		double c = -0.5*len_AB*std::cos(2*alpha - phi_y)/std::sin(2*alpha) + len_BP * (sin_beta_ABP * coeff_sin - (cos_beta_ABP) * coeff_cos) / len_PQ;
		// solve the equation
		eqn_eval = [=](double theta) -> double 
					{
					   	return a*std::cos(2*theta + phi_y) + b*std::sin(theta + phi_y + phi_w) + c;
					};
		eqn_diff = [=](double theta) -> double
					{
					   	return -2*a*std::sin(2*theta + phi_y) + b*std::cos(theta + phi_y + phi_w);
					};

		double step = pi/10;
		std::list<double> eqn_roots;
		for (double x = -pi; x <= pi; x += step)
		{
			std::pair<double, size_t> slv_res = solve_eqn(eqn_eval, eqn_diff, x);
			if (slv_res.second < 10000)
			{
				eqn_roots.push_back(slv_res.first);
			}
		}	
		//int nb_roots = 0;
		// for debug
		std::function<double(double)> collinear_check = [=](double root) -> double
														{
															double sn = std::sin(root), cn = std::cos(root);
															double tmpx = 0.5*len_AB*std::sin(alpha + root)/std::sin(alpha);
															double tmpy = 0.5*len_AB*std::sin(alpha + root)/std::cos(alpha);
															double px = tmpx + len_BP*(cos_beta_ABP*cn - sin_beta_ABP*sn);
															double py = tmpy + len_BP*(sin_beta_ABP*cn + cos_beta_ABP*sn);
															double qx = tmpx + len_BQ*(cos_beta_ABQ*cn - sin_beta_ABQ*sn);
															double qy = tmpy + len_BQ*(sin_beta_ABQ*cn + cos_beta_ABQ*sn);
															Point_2 lp(px, py), lq(qx, qy);
															Vector_2 lwp(lw, lp), lwq(lw, lq);
															return std::fabs(lwp.x()*lwq.y() - lwp.y()*lwq.x());
														};
		std::function< std::pair<Point_2, Point_2>(double) > local_point = [=](double root)
																		   {
																				double sn = std::sin(root), cn = std::cos(root);
																				double tmpx = 0.5*len_AB*std::sin(alpha + root)/std::sin(alpha);
																				double tmpy = 0.5*len_AB*std::sin(alpha + root)/std::cos(alpha);
																				double px = tmpx + len_BP*(cos_beta_ABP*cn - sin_beta_ABP*sn);
																				double py = tmpy + len_BP*(sin_beta_ABP*cn + cos_beta_ABP*sn);
																				double qx = tmpx + len_BQ*(cos_beta_ABQ*cn - sin_beta_ABQ*sn);
																				double qy = tmpy + len_BQ*(sin_beta_ABQ*cn + cos_beta_ABQ*sn);
																				Point_2 lp(px, py), lq(qx, qy);
																				return std::make_pair(lp, lq);
																		    };
		//std::list< std::pair<Point_2, Point_2> > local_pnts;
		//std::cout<<"=============================\n";
		for (std::list<double>::const_iterator it = eqn_roots.begin(); it != eqn_roots.end(); ++it)
		{
			if (-alpha < *it && *it < alpha)
			{
				std::list<double>::iterator rit;
				for (rit = roots.begin(); rit != roots.end(); ++rit)
				{
					if (std::fabs(*rit - *it) < 1e-6)
						break;
				}
				if (rit == roots.end())
					roots.push_back(*it);
			}
		}

		//assert(nb_roots <= 2);
		// check whether the intersection point is on the segment
		std::function<bool(double)> on_segment = [=](double root) -> bool
													{
														double sn = std::sin(root), cn = std::cos(root);
														double tmpx = 0.5*len_AB*std::sin(alpha + root)/std::sin(alpha);
														double tmpy = 0.5*len_AB*std::sin(alpha + root)/std::cos(alpha);
														double px = tmpx + len_BP*(cos_beta_ABP*cn - sin_beta_ABP*sn);
														double py = tmpy + len_BP*(sin_beta_ABP*cn + cos_beta_ABP*sn);
														double qx = tmpx + len_BQ*(cos_beta_ABQ*cn - sin_beta_ABQ*sn);
														double qy = tmpy + len_BQ*(sin_beta_ABQ*cn + cos_beta_ABQ*sn);
														Point_2 lp(px, py), lq(qx, qy);
														return Vector_2(lw, lp)*Vector_2(lw, lq) <= 0;
													};
		std::list<double>::iterator it = roots.begin();
		while (it != roots.end())
		{
			if (!on_segment(*it))
			{
				it = roots.erase(it);
			}
			else
				++it;
		}
	
		return roots.size();
	}

	void Polygon_containment::detect_contacts(const CGAL_polygon& support_pgn, const CGAL_polygon& embed_pgn, std::list<CGAL_polygon>& placed_polygons)
	{
		// prepare all edge pairs of interior and exterior
		static int iter = 0;
		std::list< std::pair<size_t, size_t> > int_edge_pairs, ext_edge_pairs;
		for (size_t i = 0; i < embed_pgn.size(); i++)
		{
			for (size_t j = 0; j < embed_pgn.size(); j++)
			{
				if (i == j)	continue;
				int_edge_pairs.push_back(std::make_pair(i, j));
			}
		}
		for (size_t i = 0; i < support_pgn.size(); i++)
		{
			for (size_t j = i + 1; j < support_pgn.size(); j++)
				ext_edge_pairs.push_back(std::make_pair(i, j));
		}
		ext_edge_pairs.push_back(std::make_pair(support_pgn.size()-1, 0));
		for (std::list< std::pair<size_t, size_t> >::const_iterator eit = ext_edge_pairs.begin(); eit != ext_edge_pairs.end(); ++eit)
		{
			// construct the coordinate system
			const Segment_2 P1Q1 = support_pgn.edge(eit->first);
			const Segment_2 P2Q2 = support_pgn.edge(eit->second);
			const Point_2 P2 = P2Q2.source();
			const Point_2 Q2 = P2Q2.target();
			CGAL::Object o = CGAL::intersection(P1Q1.supporting_line(), P2Q2.supporting_line());
			
			if (const Point_2 *ipnt = CGAL::object_cast<Point_2>(&o))
			{
				Point_2 O = *ipnt;
				double alpha = P1Q1.opposite().to_vector()*P2Q2.to_vector()/(std::sqrt(P1Q1.squared_length()*P2Q2.squared_length()));
				alpha = std::acos(alpha);
				alpha = (pi - alpha)/2;
				Vector_2 Ox;
				if (CGAL::squared_distance(O, P2) > CGAL::squared_distance(O, Q2))
				{
					Ox = Vector_2(CGAL::ORIGIN, Point_2(P2.x() - O.x(), P2.y() - O.y()));
					//Ox = Ox.transform(Transformation_2(CGAL::ROTATION, std::sin(alpha), std::cos(alpha)));
				}
				else
				{
					Ox = Vector_2(CGAL::ORIGIN, Point_2(Q2.x() - O.x(), Q2.y() - O.y()));
					//Ox = Ox.transform(Transformation_2(CGAL::ROTATION, std::sin(-alpha), std::cos(-alpha)));
				}
				Ox = Ox.transform(Transformation_2(CGAL::ROTATION, std::sin(-alpha), std::cos(-alpha)));
				Vector_2 Oy = Ox.perpendicular(CGAL::COUNTERCLOCKWISE);
				Local_frame lf(O, Ox, Oy);
				for (std::list< std::pair<size_t, size_t> >::const_iterator iit = int_edge_pairs.begin(); iit != int_edge_pairs.end(); ++iit)
				{
					const Point_2 A = embed_pgn.vertex(iit->first);
					const Point_2 B = embed_pgn.vertex(iit->second);
					Segment_2 AB(A, B);
					if (!is_clamped(P1Q1, P2Q2, AB))
						continue;
					// type I contact detection
					for (size_t i = 0; i < embed_pgn.size(); i++)
					{
						if (i == iit->first || i == iit->second) 
							continue;
						for (size_t j = 0; j < support_pgn.size(); j++)
						{
							double theta_1, theta_2;
							Point_2 C = embed_pgn.vertex(i);
							Segment_2 PQ = support_pgn.edge(j);
							int nb_theta = type_I_contact_detect(lf.to_local_coord(AB), lf.to_local_coord(Segment_2(B, C)), lf.to_local_coord(support_pgn.edge(j)), alpha, theta_1, theta_2);	
							//Point_2 tC_1, tC_2;
							//int nb_theta = type_I_contact_detect(lf, AB, Segment_2(B, C), PQ, alpha, theta_1, theta_2, tC_1, tC_2);	
							if (nb_theta == 1)
							{
								assert(theta_1 >= -alpha && theta_1 <= alpha);
								//Transformation_2 t;
								CGAL_polygon pgn;
								//placement.push_back(determine_transformation(P1Q1, P2Q2, AB, Ox, theta_1));
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, theta_1, alpha, pgn))
								{
									//std::cout<<iit->first<<','<<iit->second<<' ';
									//std::cout<<iter++<<":I-1\n";
									placed_polygons.push_back(pgn);
								}
							}
							else if (nb_theta == 2)
							{
								assert(theta_1 >= -alpha && theta_1 <= alpha);
								//placement.push_back(determine_transformation(P1Q1, P2Q2, AB, Ox, theta_1));
								//Transformation_2 t;
								CGAL_polygon pgn;
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, theta_1, alpha, pgn))
								{
									//std::cout<<iit->first<<','<<iit->second<<' ';
									//std::cout<<iter++<<":I-2\n";
									placed_polygons.push_back(pgn);
								}
								assert(theta_2 >= -alpha && theta_2 <= alpha);
								//placement.push_back(determine_transformation(P1Q1, P2Q2, AB, Ox, theta_2));
								pgn.clear();
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, theta_2, alpha, pgn))
								{
									//std::cout<<iit->first<<','<<iit->second<<' ';
									//std::cout<<iter++<<":I-2\n";
									placed_polygons.push_back(pgn);
								}
							}
						}
					}
					// type II contact detection
					for (size_t i = 0; i < embed_pgn.size(); i++)
					{
						Segment_2 PQ = embed_pgn.edge(i);
						Point_2 P = PQ.source(), Q = PQ.target();
						if (P == A || P == B || Q == A || Q == B) 
							continue;
						for (size_t j = 0; j < support_pgn.size(); j++)
						{
							//double theta_1, theta_2;
							std::list<double> roots;
							Point_2 w = support_pgn.vertex(j);
							int nb_theta = type_II_contact_detect(lf, w, AB, PQ, alpha, roots);
							for (std::list<double>::const_iterator it = roots.begin(); it != roots.end(); ++it)
							{
								assert(*it >= -alpha && *it <= alpha);
								CGAL_polygon pgn;
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, *it, alpha, pgn))
								{
									//std::cout<<iter++<<":II\n";
									placed_polygons.push_back(pgn);
								}
							}
						}
					}
				}
			}
			//else // handle the parallel case, we do not consider the case in the version, refer to backup.txt for recovering
			//{}
		}
	}

	bool Polygon_containment::detect_contacts_and_check_inclusion(const CGAL_polygon& support_pgn, const CGAL_polygon& embed_pgn, bool exterior_is_support, CGAL_polygon& placed_polygon)
	{
		// prepare all edge pairs of interior and exterior
		static int iter = 0;
		std::list< std::pair<size_t, size_t> > int_edge_pairs, ext_edge_pairs;
		for (size_t i = 0; i < embed_pgn.size(); i++)
		{
			for (size_t j = 0; j < embed_pgn.size(); j++)
			{
				if (i == j)	continue;
				int_edge_pairs.push_back(std::make_pair(i, j));
			}
		}
		for (size_t i = 0; i < support_pgn.size(); i++)
		{
			for (size_t j = i + 1; j < support_pgn.size(); j++)
				ext_edge_pairs.push_back(std::make_pair(i, j));
		}
		ext_edge_pairs.push_back(std::make_pair(support_pgn.size()-1, 0));
		// lambda function just to reduce code redundancy
		std::function<bool(const CGAL_polygon&)> check_inclusion = [&](const CGAL_polygon& transformed_pgn)
													{
														if (exterior_is_support)
															return has_inclusion(support_pgn, transformed_pgn);
														else
															return has_inclusion(transformed_pgn, support_pgn);
													};
		//const size_t N = 1000;
		//size_t nb_test = 0;
		for (std::list< std::pair<size_t, size_t> >::const_iterator eit = ext_edge_pairs.begin(); eit != ext_edge_pairs.end(); ++eit)
		{
			// construct the coordinate system
			const Segment_2 P1Q1 = support_pgn.edge(eit->first);
			const Segment_2 P2Q2 = support_pgn.edge(eit->second);
			const Point_2 P2 = P2Q2.source();
			const Point_2 Q2 = P2Q2.target();
			CGAL::Object o = CGAL::intersection(P1Q1.supporting_line(), P2Q2.supporting_line());
			
			if (const Point_2 *ipnt = CGAL::object_cast<Point_2>(&o))
			{
				Point_2 O = *ipnt;
				double alpha = P1Q1.opposite().to_vector()*P2Q2.to_vector()/(std::sqrt(P1Q1.squared_length()*P2Q2.squared_length()));
				alpha = std::acos(alpha);
				alpha = (pi - alpha)/2;
				Vector_2 Ox;
				if (CGAL::squared_distance(O, P2) > CGAL::squared_distance(O, Q2))
					Ox = Vector_2(CGAL::ORIGIN, Point_2(P2.x() - O.x(), P2.y() - O.y()));
				else
					Ox = Vector_2(CGAL::ORIGIN, Point_2(Q2.x() - O.x(), Q2.y() - O.y()));
				Ox = Ox.transform(Transformation_2(CGAL::ROTATION, std::sin(-alpha), std::cos(-alpha)));
				Vector_2 Oy = Ox.perpendicular(CGAL::COUNTERCLOCKWISE);
				Local_frame lf(O, Ox, Oy);
				for (std::list< std::pair<size_t, size_t> >::const_iterator iit = int_edge_pairs.begin(); iit != int_edge_pairs.end(); ++iit)
				{
					const Point_2 A = embed_pgn.vertex(iit->first);
					const Point_2 B = embed_pgn.vertex(iit->second);
					Segment_2 AB(A, B);
					if (!is_clamped(P1Q1, P2Q2, AB))
						continue;
					// type I contact detection
					for (size_t i = 0; i < embed_pgn.size(); i++)
					{
						if (i == iit->first || i == iit->second) 
							continue;
						for (size_t j = 0; j < support_pgn.size(); j++)
						{
							double theta_1, theta_2;
							Point_2 C = embed_pgn.vertex(i);
							Segment_2 PQ = support_pgn.edge(j);
							int nb_theta = type_I_contact_detect(lf.to_local_coord(AB), lf.to_local_coord(Segment_2(B, C)), lf.to_local_coord(support_pgn.edge(j)), alpha, theta_1, theta_2);	
							if (nb_theta == 1)
							{
								assert(theta_1 >= -alpha && theta_1 <= alpha);
								CGAL_polygon pgn;
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, theta_1, alpha, pgn))
								{
									if (check_inclusion(pgn))	
									{
										placed_polygon = pgn;
										return true;
									}
								}
							}
							else if (nb_theta == 2)
							{
								assert(theta_1 >= -alpha && theta_1 <= alpha);
								CGAL_polygon pgn;
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, theta_1, alpha, pgn))
								{
									if (check_inclusion(pgn))	
									{
										placed_polygon = pgn;
										return true;
									}
								}
								assert(theta_2 >= -alpha && theta_2 <= alpha);
								pgn.clear();
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, theta_2, alpha, pgn))
								{
									if (check_inclusion(pgn))	
									{
										placed_polygon = pgn;
										return true;
									}
								}
							}
							//nb_test++;
							//if (nb_test > N)
							//	return false;
						}
					}
					// type II contact detection
					for (size_t i = 0; i < embed_pgn.size(); i++)
					{
						Segment_2 PQ = embed_pgn.edge(i);
						Point_2 P = PQ.source(), Q = PQ.target();
						if (P == A || P == B || Q == A || Q == B) 
							continue;
						for (size_t j = 0; j < support_pgn.size(); j++)
						{
							//double theta_1, theta_2;
							std::list<double> roots;
							Point_2 w = support_pgn.vertex(j);
							int nb_theta = type_II_contact_detect(lf, w, AB, PQ, alpha, roots);
							for (std::list<double>::const_iterator it = roots.begin(); it != roots.end(); ++it)
							{
								assert(*it >= -alpha && *it <= alpha);
								CGAL_polygon pgn;
								if (determine_transformation(lf, embed_pgn, P1Q1, P2Q2, A, B, *it, alpha, pgn))
								{
									if (check_inclusion(pgn))	
									{
										placed_polygon = pgn;
										return true;
									}
								}
							}
							//nb_test++;
							//if (nb_test > N)
							//	return false;
						}
					}
				}
			}
			//else // handle the parallel case, we do not consider the case in the version, refer to backup.txt for recovering
			//{}
		}
		return false;		
	}
	bool Polygon_containment::determine_transformation(const Local_frame& lf, const CGAL_polygon& embed_pgn, const Segment_2& fixed_edge_1, 
														const Segment_2& fixed_edge_2, const Point_2& A, const Point_2& B, 
														double theta, double alpha, CGAL_polygon& transformed_polygon)
	{
		Point_2 P1 = fixed_edge_1.source(), P2 = fixed_edge_1.target();
		Point_2 Q1 = fixed_edge_2.source(), Q2 = fixed_edge_2.target();
		Vector_2 Ox = lf.u_axis();
		Segment_2 AB(A, B);
		// coefficients of the quadratic form
		Vector_2 P1P2(P1, P2), Q1Q2(Q1, Q2), P1Q1(P1, Q1);
		gsl_matrix *M = gsl_matrix_alloc(2, 2);
		gsl_matrix_set(M, 0, 0, (P1.x() - P2.x())*Ox.x() + (P1.y() - P2.y())*Ox.y());
		gsl_matrix_set(M, 0, 1, (Q2.x() - Q1.x())*Ox.x() + (Q2.y() - Q1.y())*Ox.y());
		gsl_matrix_set(M, 1, 0, (P1.x() - P2.x())*Ox.y() + (P2.y() - P1.y())*Ox.x());
		gsl_matrix_set(M, 1, 1, (Q2.x() - Q1.x())*Ox.y() + (Q1.y() - Q2.y())*Ox.x());
		gsl_vector *b = gsl_vector_alloc(2);
		gsl_vector_set(b, 0, -(Q1.x() - P1.x())*Ox.x() - (Q1.y() - P1.y())*Ox.y() + std::sqrt(AB.squared_length()*Ox.squared_length())*std::cos(theta));
		gsl_vector_set(b, 1, -(Q1.x() - P1.x())*Ox.y() - (P1.y() - Q1.y())*Ox.x() + std::sqrt(AB.squared_length()*Ox.squared_length())*std::sin(-theta));
		gsl_permutation *p = gsl_permutation_alloc(2);
		gsl_vector *x = gsl_vector_alloc(2);
		int s;
		gsl_linalg_LU_decomp(M, p, &s);
		gsl_linalg_LU_solve(M, p, b, x);

		double wa = gsl_vector_get(x, 0), wb = gsl_vector_get(x, 1);

		gsl_permutation_free(p);
		gsl_matrix_free(M);
		gsl_vector_free(b);
		gsl_vector_free(x);

		if (wa < 0.0 || wa > 1.0 || wb < 0.0 || wb > 1.0)
			return false;
			
		Point_2 tA = CGAL::ORIGIN + (1 - wa)*Vector_2(CGAL::ORIGIN, P1) + wa*Vector_2(CGAL::ORIGIN, P2);
		Point_2 tB = CGAL::ORIGIN + (1 - wb)*Vector_2(CGAL::ORIGIN, Q1) + wb*Vector_2(CGAL::ORIGIN, Q2);
		//Point_2 lA = lf.to_local_coord(A), lB = lf.to_local_coord(A);
		double len_AB = std::sqrt(CGAL::squared_distance(A, B));
		//double len_tAB = std::sqrt(CGAL::squared_distance(tA, tB));
		//std::cout<<len_AB<<','<<len_tAB<<std::endl;
		double tan_alpha = std::tan(alpha);
		//Point_2 dbA, dbB;
		for (CGAL_polygon::Vertex_const_iterator vit = embed_pgn.vertices_begin(); vit != embed_pgn.vertices_end(); ++vit)
		{
			Point_2 C = *vit;
			if (C == A)
			{
				transformed_polygon.push_back(tA);
				//std::cout<<"A("<<tA.x()<<','<<tA.y()<<"), ";
				//dbA = tA;
				continue;
			}
			if (C == B)
			{
				transformed_polygon.push_back(tB);
				//std::cout<<"B("<<tB.x()<<','<<tB.y()<<")\n";
				//dbB = tB;
				continue;
			}
			//Point_2 lC = lf.to_local_coord(*vit);
			Segment_2 BC(B, C);
			double len_BC = std::sqrt(CGAL::squared_distance(B, C));
			// double cos_beta = AB.to_vector()*BC.to_vector()/(len_AB*len_BC);
			// double sin_beta = std::sqrt(1.0 - cos_beta*cos_beta);
			// if ((B.x()-A.x())*(C.y() - B.y()) - (B.y()-A.y())*(C.x()-B.x()) < 0)
			// 	sin_beta = -sin_beta;
			double cos_beta, sin_beta;
			compute_sin_cos(AB.to_vector(), BC.to_vector(), sin_beta, cos_beta);
			double ax =	0.5*len_AB/tan_alpha - len_BC*sin_beta, bx = 0.5*len_AB + len_BC*cos_beta;
			double ay = 0.5*len_AB + len_BC*cos_beta, by = 0.5*len_AB*tan_alpha + len_BC*sin_beta;
			double sn = std::sin(theta), cn = std::cos(theta);
			double x = ax*sn + ay*cn, y = bx*sn + by*cn;
			Point_2 ipnt = lf.to_world_coord(Point_2(x, y));
			transformed_polygon.push_back(ipnt);
		}
		//double len_tAB = std::sqrt(CGAL::squared_distance(dbA, dbB));
		//std::cout<<len_AB<<','<<len_tAB<<std::endl;

		return true;	
	}
		
	void Polygon_containment::contact_polygons(std::list<CGAL_polygon>& placed_polygons)
	{
		// detect the contacts from interior to exterior
		detect_contacts(exterior, interior, placed_polygons);
		// according to symmetry, detect the contacts from exterior to interior
		std::list<CGAL_polygon> placed_ext_polygons;
		detect_contacts(interior, exterior, placed_ext_polygons);
		
		Point_2 c = CGAL::centroid(exterior.vertices_begin(), exterior.vertices_end(), CGAL::Dimension_tag<0>());
		Vector_2 u(c, exterior.vertex(0));
		Vector_2 v = u.perpendicular(CGAL::COUNTERCLOCKWISE);
		Local_frame lf(c, u, v);
		
		for (std::list<CGAL_polygon>::const_iterator it = placed_ext_polygons.begin(); it != placed_ext_polygons.end(); ++it)
		{
			// local coordinate system formed by the moved exterior polygons
			Point_2 invc = CGAL::centroid(it->vertices_begin(), it->vertices_end(), CGAL::Dimension_tag<0>());
			Vector_2 invu(invc, it->vertex(0));
			Vector_2 invv = invu.perpendicular(CGAL::COUNTERCLOCKWISE);	
			Local_frame invlf(invc, invu, invv);
			
			// compute the coordinates of an interior polygon in the local coordinate system 	
			std::vector<Point_2> local_int_verts(interior.size());
			std::transform(interior.vertices_begin(), interior.vertices_end(), local_int_verts.begin(), [=](const Point_2& p){return invlf.to_local_coord(p);});
			
			// compute the transformed interior polygon into world system 
			CGAL_polygon transformed_int_pgn;
			for (size_t i = 0; i < local_int_verts.size(); i++)
				transformed_int_pgn.push_back(lf.to_world_coord(local_int_verts[i]));
			
			placed_polygons.push_back(transformed_int_pgn);
		}
	}
	
	bool Polygon_containment::has_inclusion(const CGAL_polygon& out_pgn, const CGAL_polygon& in_pgn)
	{
		Point_2 c = CGAL::centroid(in_pgn.vertices_begin(), in_pgn.vertices_end(), CGAL::Dimension_tag<0>());
		Transformation_2 shrink(CGAL::SCALING, 0.99);
		Transformation_2 to_orig(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN));
		Transformation_2 to_cent(CGAL::TRANSLATION, Vector_2(CGAL::ORIGIN, c));
		CGAL_polygon shrunk_in_pgn = CGAL::transform(to_cent*shrink*to_orig, in_pgn);

		for (size_t i = 0; i < shrunk_in_pgn.size(); i++)
			if (out_pgn.has_on_unbounded_side(shrunk_in_pgn.vertex(i)))
				return false;		

		for (CGAL_polygon::Edge_const_iterator oit = out_pgn.edges_begin(); oit != out_pgn.edges_end(); ++oit)
		{
			for (CGAL_polygon::Edge_const_iterator iit = shrunk_in_pgn.edges_begin(); iit != shrunk_in_pgn.edges_end(); ++iit)
			{
				CGAL::Object o = CGAL::intersection(*oit, *iit);
				if (const Point_2 *ipnt = CGAL::object_cast<Point_2>(&o))
					return false;
			}
		}
	
		return true;
	}		
	
	bool Polygon_containment::one_containment_copy(CGAL_polygon& placed_polygon)
	{
		if (detect_contacts_and_check_inclusion(exterior, interior, true, placed_polygon))
			return true;

		// according to symmetry, detect the contacts from exterior to interior
		CGAL_polygon placed_ext_polygons;
		if (detect_contacts_and_check_inclusion(interior, exterior, false, placed_ext_polygons))
		{
			// the original local frame of exterior polygon
			Point_2 c = CGAL::centroid(exterior.vertices_begin(), exterior.vertices_end(), CGAL::Dimension_tag<0>());
			Vector_2 u(c, exterior.vertex(0));
			Vector_2 v = u.perpendicular(CGAL::COUNTERCLOCKWISE);
			Local_frame lf(c, u, v);
			// the local frame of transformed exterior polygon
			Point_2 invc = CGAL::centroid(placed_ext_polygons.vertices_begin(), placed_ext_polygons.vertices_end(), CGAL::Dimension_tag<0>());
			Vector_2 invu(invc, placed_ext_polygons.vertex(0));
			Vector_2 invv = invu.perpendicular(CGAL::COUNTERCLOCKWISE);	
			Local_frame invlf(invc, invu, invv);
			
			// compute the coordinates of an interior polygon in the local coordinate system 	
			std::vector<Point_2> local_int_verts(interior.size());
			std::transform(interior.vertices_begin(), interior.vertices_end(), local_int_verts.begin(), [=](const Point_2& p){return invlf.to_local_coord(p);});
			
			// compute the transformed interior polygon into world system 
			CGAL_polygon transformed_int_pgn;
			for (size_t i = 0; i < local_int_verts.size(); i++)
				transformed_int_pgn.push_back(lf.to_world_coord(local_int_verts[i]));
			
			placed_polygon = transformed_int_pgn;
			return true;			
		}
		
		return false;
	}
	
	int Polygon_containment::all_containment_copies(std::list<CGAL_polygon>& placed_polygons)
	{
		contact_polygons(placed_polygons);
		std::list<CGAL_polygon>::iterator it = placed_polygons.begin();
		while (it != placed_polygons.end())
		{
			if (!has_inclusion(exterior, *it))
				it = placed_polygons.erase(it);
			else
				++it;
		}
		return placed_polygons.size();
	}
}